package com.asa.control_theory;

import com.asa.hutils.MathHUtils;
import com.asa.hutils.Ni;

public class F {
	
	/**
	 * Advanced控制理论
	 * 
	 * LQR 控制器							我没有看懂它的原理是什么，也就是我没有办法实现它
	 * linear Quadratic Regulater
	 * 
	 * 
	 * 	开环	X` = A*X
	 * 	闭环	X` = A*X + B*u
	 * 	u = -K * X =-{k1,k2,k3...} * 
	 * 				{
	 * 				{X1},
	 * 				{X2},
	 * 				{X3},
	 * 				.
	 * 				.
	 * 				.
	 * 				}
	 * 		X` = A*X - B*k*X = (A-B*k)*X	②
	 * 	
	 * 		那么，选择k--》改变 Acl的特征值 --》控制系统状况
	 * 		
	 * 		·如何确定特征值λi？
	 * 		·什么样的特征值才是好的？
	 * 
	 * 		引入 Cost Function （目标函数，能量函数，代价函数）
	 * 		J = 1/2* 积分【0→∞】(X(T)*Q*X + U(T)*R*U)dt			Q这个矩阵是正的，对角矩阵的对角线上的值都是正的	
	 * 			a*x1*x1 + b*x2*x2 + c*x3*x3 + 	...			
	 * 		J = 1/2* 积分【0→∞】(X(T)*(Q + K(T)*R*K)*X)dt	③
	 * 		
	 * 		设存在一个常量矩阵P使得：
	 * 		d(X(T)*P*X)/dt = X(T)*(Q + K(T)*R*K)*X		④
	 * 
	 * 		将④代入到③
	 * 		J = -1/2 *积分【0→∞】(d(X(T)*P*X)/dt) dt = 1/2 * X(T)*P*X ===> 1/2 * X0(T)*P*X0		⑤
	 * 	
	 * 		注意，我们已经假设闭环系统是稳定的，也就是t趋于无穷时，x(t)趋于0.
	 * 		现在把④式左边的微分展开，并把状态变量x的微分用②式替代得到：
	 * 		X(T)*(A(T)*P + P*A + Q + K(T)*R*K)*X = 0
	 * 		
	 * 		这个式子要始终成立的话，括号里的项必须恒等于0.
	 * 		A(T)*P + P*A + Q + K(T)*R*K = 0
	 * 		
	 * 		这是一个关于K的二次型等式，当然这个二次型是我们不愿看到的，因为计算复杂。现在只要这个等式成立，
	 * 		我们何必不选择K使得两个二次项正好约掉了呢？这样既符合了要求，又简化了计算。
	 * 		取  K = R(-1)*B(T)*P  代入上式得：
	 * 		A(T)*P + P*A + Q + P*B*R(-1)*B(T)*P = 0		⑥
	 * 		
	 * 		K的二次项没有了，可K的取值和P有关，而P是我们假设的一个量，
	 * 		P只要使得的⑥式成立就行了。而⑥式在现代控制理论中极其重要，它就是有名的Riccati 方程。
	 * 
	 * 
	 * 
	 * 		min J 取最小的值，			simulink--MATLAB
	 * 
	 * 		对于 J 
	 * 			Q	它是对角矩阵，并且它的对角线上的值都是正的，Q上对应的x的每一个值越大，对应的x的权重就越大
	 * 			R	它是对角矩阵，对角线上同一个值？？，R越大输入的影响就越大	R越小输入的影响就越小
	 * 		
	 * 		Q和R都是自定义设置的
	 * 	
	 * 
	 * 
	 * 
	 * 		例子:
	 * 		A = {{0,1},
	 * 			{10,0}}		
	 * 		
	 * 		B = {
	 * 			{0},
	 * 			{1}
	 * 			}
	 * 		
	 * 		Q = {{100,0},
	 * 			{0,1}
	 * 			}		
	 * 	
	 * 		R = 0.1
	 * 		
	 * 		k = lqr(A,B,Q,R)
	 * 		得出：110.4988 	17.916
	 * 
	 * 		
	 * 		
	 * 
	 * 
	 * 
	 */
	
	
	/**
	 * Riccati方程迭代法求解
	 * 	方程查看control_theory-F01.png这个图片
	 * 	让我感到迷惑的是PN的初始条件Qf，是否就是传入的Q
	 * @param A
	 * @param B
	 * @param Q
	 * @param R
	 * @return
	 * @throws Exception 
	 */
	public static double[][][] Riccati(double[][] A,double[][] B,double[][] Q,double[][] R) throws Exception{
		double[][][] P = new double[A.length+1][][];
		P[P.length-1] = Q;
		
		for (int t = P.length-1; t >0; t++) {
			double[][] asa = Q;
			double[][] asb = MathHUtils.chenfa(MathHUtils.zhuanzhi(A), P[t]);
			asb = MathHUtils.chenfa(asb, A);
			asa = MathHUtils.jiafa(asa, asb);
			
			asb = MathHUtils.chenfa(MathHUtils.zhuanzhi(A), P[t]);
			asb = MathHUtils.chenfa(asb, B);
			double[][] asc = R;
			double[][] asd = MathHUtils.chenfa(MathHUtils.zhuanzhi(B), P[t]);
			asd = MathHUtils.chenfa(asd, B);
			asc = MathHUtils.jiafa(asc, asd);
			
			asc = MathHUtils.juzhengni(asc);
			
			asb = MathHUtils.chenfa(asb, asc);
			
			asb = MathHUtils.chenfa(asb, MathHUtils.zhuanzhi(B));
			asb = MathHUtils.chenfa(asb, P[t]);
			asb = MathHUtils.chenfa(asb, A);

			
			
			P[t-1] = MathHUtils.jiafa(asa, asb);
			
		}
		
		
		
		return P;
	}

	
	
	
	
	/**
	 * 	这个和matlab的不一样，有点
	 * @param P
	 * @param A
	 * @param B
	 * @param Q
	 * @param R
	 * @return
	 * @throws Exception
	 */
	public static double[][][] Kt(double[][][] P,double[][] A,double[][] B,double[][] Q,double[][] R) throws Exception{
		double[][][] K = new double[P.length - 1][][];
		
		for (int t = 0; t < P.length - 1; t++) {
			
			double[][] asa = R;
			double[][] asb = MathHUtils.zhuanzhi(B);
			asb = MathHUtils.chenfa(asb, P[t+1]);
			asb = MathHUtils.chenfa(asb, B);
			asa = MathHUtils.jiafa(asa, asb);
			asa = MathHUtils.juzhengni(asa);
			
			asa = MathHUtils.chenfa(asa, MathHUtils.zhuanzhi(B));
			asa = MathHUtils.chenfa(asa, P[t+1]);
			asa = MathHUtils.chenfa(asa, A);
			asa = MathHUtils.chenfa(asa, -1);
			
			
			K[t] = asa;	
		}
		return K;
	}

	
	
	
	
	
	
	
	
	
	
	
	
}
